#!/usr/bin/python3

import rospy
import tf
from geometry_msgs.msg import PointStamped
from pointsenter import Sender

class PointCalculation():
    def __init__(self) :
        self.s =  Sender()
        self.listener = tf.TransformListener() 

    def pointcal(self,list_box):
        x,y,z = list_box
        laser_pos = PointStamped() 
        laser_pos.header.frame_id = "camera_color_optical_frame" 
        laser_pos.header.stamp = rospy.Time()
        laser_pos.point.x = x
        laser_pos.point.y = y
        laser_pos.point.z = z

        self.listener.waitForTransform("camera_color_optical_frame", "panda_link0", rospy.Time(0), rospy.Duration(3))
        base_pos = self.listener.transformPoint("panda_link0", laser_pos) 
        x,y,z = base_pos.point.x, base_pos.point.y, base_pos.point.z
        # self.s.sent([x,y,z])
        reslut = [x,y,z]
        return reslut

if __name__ == '__main__':

    
    rospy.init_node('tf_subscriber') 
    p = PointCalculation()
    while not rospy.is_shutdown():
        print(p.pointcal([0,0,0.2]))